// --------------------------------------------------------------------------------------------------------------------
// <copyright file="KukaLBR4InverseKinematicsTests.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Hardware.KukaLBR.KukaLBR4Tests
{
    using System;
    using System.Collections.Generic;

    using Microsoft.Robotics.Hardware.KukaLBR;
    using Microsoft.Robotics.Kinematics;
    using Microsoft.Robotics.Manipulation;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    using ShoNS.Array;

    /// <summary>
    /// Test inverse kinematics
    /// </summary>
    [TestClass]
    public class KukaLBR4InverseKinematicsTests
    {
        /// <summary>
        /// Test inverse kinematics for the 6 DOF Kuka arm using the closed-form approach developed by Kuka
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void InverseKinematicsTestKuka6DOF()
        {
            KinematicChain kc = KukaLBR4KinematicConfigurations.CreateKuka6DOFKinematicChain();

            // generate random joint angles, compute the end effector pose using forward
            // kinematics and solve the IK based on the end effector pose
            for (int i = 0; i < 10; ++i)
            {
                // generate one joint sample
                DoubleArray jointSample = InverseKinematicsNumerical.GenerateRandomJointConfiguration(kc, 100, i + 1);

                // calculate end effector pose using forward kinematics
                Matrix4[] kinematics = kc.GetJointTransforms(jointSample);
                Matrix4[] kinematicsOriginal = kinematics;

                // feed the end effector pose into the KUKA dedicated IK solver
                List<DoubleArray> solutions = KukaLBR4IK6DOFClosedForm.DedicatedIK(kc, kinematics[6]);

                // validate that the eight solutions are correct
                for (int j = 0; j < 8; ++j)
                {
                    // compute the end effector pose using forward kinematics
                    Matrix4[] kinematicsValidate = kc.GetJointTransforms(solutions[j]);

                    // record test related information
                    string msg_joint = string.Format("failed solution for original joint sample [{0}, {1}, {2}, {3}, {4}, {5}]\n", jointSample[0], jointSample[1], jointSample[2], jointSample[3], jointSample[4], jointSample[5]);
                    string msg_solution_index = string.Format("failed solution index: {0}\n", j);
                    string msg_solutions = "all solutions generated: " + KukaLBR4KinematicsTestConfigurations.PrintOutSolutions(solutions);
                    string msg = msg_joint + msg_solution_index + msg_solutions;

                    PoseDiff diff = new PoseDiff(kinematicsOriginal[6], kinematicsValidate[6]);
                    Assert.AreEqual(MatrixExtensions.MaxAbsoluteValue(diff), 0, 1e-3, msg);
                }
            }
        }

        /// <summary>
        /// Test inverse kinematics for the 6 DOF Kuka arm using the closed-form approach developed by Kuka
        /// Comparing the results against the IK solutions from the general IK solver
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void InverseKinematicsKuka6DOFConsistencyTest()
        {
            KinematicChain kc = KukaLBR4KinematicConfigurations.CreateKuka6DOFKinematicChain();

            // generate random joint angles, compute the end effector pose using forward
            // kinematics and solve the IK based on the end effector pose
            for (int i = 0; i < 3; ++i)
            {
                // generate one joint sample
                DoubleArray jointSample = InverseKinematicsNumerical.GenerateRandomJointConfiguration(kc, 100, i + 1);

                // calculate end effector pose using forward kinematics
                Matrix4[] kinematics = kc.GetJointTransforms(jointSample);
                Matrix4[] kinematicsOriginal = kinematics;

                // feed the end effector pose into the KUKA dedicated IK solver
                List<DoubleArray> solutionsClosedForm = KukaLBR4IK6DOFClosedForm.DedicatedIK(kc, kinematics[6]);
                bool maxIterationExceeded, jointWithinRange;

                // feed the end effector pose into the generalIK solver
                DoubleArray seedSample = InverseKinematicsNumerical.GenerateRandomJointConfiguration(kc, 100, i + 100);
                DoubleArray solutionGeneralIK = InverseKinematicsNumerical.SolveIKNumerical(kc, kinematics[6], seedSample, out maxIterationExceeded, out jointWithinRange);

                // try multiple times in case being trapped into local minima and could not find a way out within a number of iterations
                for (int k = 1; k < 100; ++k)
                {
                    if (!maxIterationExceeded)
                    {
                        break;
                    }

                    seedSample = InverseKinematicsNumerical.GenerateRandomJointConfiguration(kc, 100, k + 1000);
                    solutionGeneralIK = InverseKinematicsNumerical.SolveIKNumerical(kc, kinematics[6], seedSample, out maxIterationExceeded, out jointWithinRange);
                }

                bool foundOneMatch = false;
                for (int m = 0; m < solutionsClosedForm.Count; ++m)
                {
                    double distance = InverseKinematicsFilter.JointConfigurationDifference(solutionsClosedForm[m], solutionGeneralIK);
                    double localErrorEpsilon = 1e-4;
                    if (distance < localErrorEpsilon)
                    {
                        foundOneMatch = true;
                    }
                }

                string msg = string.Format("{0}", i);
                Assert.IsTrue(foundOneMatch, msg);
            }
        }

        /// <summary>
        /// Test inverse kinematics for the 7 DOF Kuka arm using the joint equivalent generator
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void InverseKinematicsEquivalentGeneratorTestKuka7DOF()
        {
            KinematicChain kc = KukaLBR4KinematicConfigurations.CreateKuka7DOFKinematicChain();

            for (int i = 0; i < 10; ++i)
            {
                // generate one joint sample
                DoubleArray jointSample = InverseKinematicsNumerical.GenerateRandomJointConfiguration(kc, 100, i + 1);

                // calculate end effector pose using forward kinematics
                Matrix4[] kinematics = kc.GetJointTransforms(jointSample);
                Matrix4[] kinematicsOriginal = kinematics;

                // generate equivalent solutions
                List<DoubleArray> solutions = KukaLBR4KinematicConfigurations.KukaLBR4EquivalentJointConfigurationGenerator(jointSample);

                // validate solutions
                for (int j = 0; j < solutions.Count; ++j)
                {
                    DoubleArray solution = solutions[j];

                    // compute the end effector pose using forward kinematics
                    Matrix4[] kinematicsValidate = kc.GetJointTransforms(solution);

                    // record test related information
                    string msg_joint = string.Format("failed solution for original joint sample [{0}, {1}, {2}, {3}, {4}, {5}, {6}]\n", jointSample[0], jointSample[1], jointSample[2], jointSample[3], jointSample[4], jointSample[5], jointSample[6]);
                    string msg_solution_index = string.Format("failed solution index: {0}\n", j);
                    string msg = msg_joint + msg_solution_index;

                    PoseDiff diff = new PoseDiff(kinematicsOriginal[6], kinematicsValidate[6]);
                    Assert.AreEqual(MatrixExtensions.MaxAbsoluteValue(diff), 0, 1e-3, msg);
                }
            }
        }
    }
}
